class AdmittanceController():
    def __init__(self, mass, damping, stiffness,goal_force):
        self.mass = mass
        self.damping = damping
        self.stiffness = stiffness
        self.prev_acceleration = 0
        self.prev_velocity = 0
        self.prev_position = 0
        self.cur_acceleration = 0
        self.cur_velocity = 0
        self.cur_position = 0
        self.goal_force = goal_force

    def calpose(self,curforce):
        if abs(curforce)<0.5:
            curforce=0
        Fe=curforce-self.goal_force
        Ve=self.prev_velocity
        Xe=self.prev_position
        self.cur_acceleration = (1/self.mass)*(Fe - self.damping*Ve - self.stiffness*Xe)
        self.cur_velocity = Ve + self.cur_acceleration*0.008

        # if abs(self.cur_velocity)>0.001:
        #     self.cur_velocity=0.001

        self.cur_position = Xe + self.cur_velocity*0.008

        self.prev_acceleration = self.cur_acceleration
        self.prev_velocity = self.cur_velocity

        return self.cur_position